// main.cpp

/* Copyright (c)  2009 Stanford University
 * Use of the OpenSim software in source form is permitted provided that the following
 * conditions are met:
 *   1. The software is used only for non-commercial research and education. It may not
 *     be used in relation to any commercial activity.
 *   2. The software is not distributed or redistributed.  Software distribution is allowed 
 *     only through https://simtk.org/home/opensim.
 *   3. Use of the OpenSim software or derivatives must be acknowledged in all publications,
 *      presentations, or documents describing work in which OpenSim or derivatives are used.
 *   4. Credits to developers may not be removed from executables
 *     created from modifications of the source.
 *   5. Modifications of source code must retain the above copyright notice, this list of
 *     conditions and the following disclaimer. 
 * 
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
 *  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 *  OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
 *  SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 *  TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
 *  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *  OR BUSINESS INTERRUPTION) OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
 *  WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/* 
 *  Below is an example of an OpenSim application that provides its own 
 *  main() routine.  This application is a forward simulation of tug-of-war between two
 *  muscles pulling on a block.
 */

// Author:  Jeff Reinbolt, Ayman Habib, Ajay Seth, Jack Middleton, Samuel Hamner

//==============================================================================
//==============================================================================
#include <OpenSim/OpenSim.h>
#include "Thelen2003Plugin\Thelen2003MuscleP1.h"


using namespace OpenSim;
using namespace SimTK;

//______________________________________________________________________________
/**
 * First exercise: create a model that does nothing. 
 */
int main()
{
	try {
		// Create an OpenSim model and set its name
		Model osimModel;
		osimModel.setName("tugOfWar");

		double initialTime=0.0, finalTime=4.0;
		
		OpenSim::Body& ground = osimModel.getGroundBody();
		ground.addDisplayGeometry("ground.vtp");
		ground.addDisplayGeometry("anchor1.vtp");
		ground.addDisplayGeometry("anchor2.vtp");

		
		double blockMass=20.0, blockSideLength=0.1;
		Vec3 blockMassCenter(0);
		Inertia blockInertia=blockMass*Inertia::brick(blockSideLength,blockSideLength,blockSideLength);
		OpenSim::Body* block = new OpenSim::Body("block",blockMass,blockMassCenter, blockInertia);
		block->addDisplayGeometry("block.vtp");

		Vec3 locationInParent(0,blockSideLength/2,0), orientationInParent(0), locationInBody(0), orientationInBody(0);
		FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
		CoordinateSet& jointCoordinateSet=blockToGround->getCoordinateSet();
		double angleRange[2]={-SimTK::Pi/2,SimTK::Pi/2};
		double positionRange[2]={-1,1};
		jointCoordinateSet[0].setRange(angleRange);
		jointCoordinateSet[1].setRange(angleRange);
		jointCoordinateSet[2].setRange(angleRange);
		jointCoordinateSet[3].setRange(positionRange);
		jointCoordinateSet[4].setRange(positionRange);
		jointCoordinateSet[5].setRange(positionRange);
		osimModel.addBody(block);
		osimModel.setGravity(Vec3(0,-9.80665,0));


		double maxIsometricForce=1000, optimalFiberLength=0.1,tendonSlackLength=0.2, pennationAngle=0.0, activation=0.0001, deactivation=1.0;
		Thelen2003MuscleP1* muscle1=new Thelen2003MuscleP1("muscle1", maxIsometricForce, optimalFiberLength, tendonSlackLength, pennationAngle);
		muscle1->setActivationTimeConstant(activation);
		muscle1->setDeactivationTimeConstant(deactivation);

		Thelen2003MuscleP1* muscle2=new Thelen2003MuscleP1("muscle2", maxIsometricForce, optimalFiberLength, tendonSlackLength, pennationAngle);
		muscle2->setActivationTimeConstant(activation);
		muscle2->setDeactivationTimeConstant(deactivation);

		muscle1->addNewPathPoint("muscle1-point1",ground,Vec3(0.0,0.05,-0.35));
		muscle1->addNewPathPoint("muscle1-point2",*block,Vec3(0.0,0.0,-0.05));

		muscle2->addNewPathPoint("muscle2-point1",ground,Vec3(0.0,0.05,0.35));
		muscle2->addNewPathPoint("muscle2-point2",*block,Vec3(0.0,0.0,0.05));

		osimModel.addForce(muscle1);
		osimModel.addForce(muscle2);

		PrescribedController *muscleController=new PrescribedController();
		muscleController->setActuators(osimModel.updActuators());
		
		ContactHalfSpace *floor=new ContactHalfSpace(Vec3(0), Vec3(0,0,-0.5*Pi), ground);
		floor->setName("floor");
		ContactMesh *cube=new ContactMesh("blockRemesh192.obj", Vec3(0), Vec3(0), *block);
		cube->setName("cube");

		osimModel.addContactGeometry(floor);
		osimModel.addContactGeometry(cube);

		OpenSim::ElasticFoundationForce *contactForce=new OpenSim::ElasticFoundationForce();
		OpenSim::ElasticFoundationForce::ContactParameters contactParams;
		contactParams.updGeometry().append("cube");
		contactParams.updGeometry().append("floor");
		contactParams.setStiffness(1.0e8);
		contactParams.setDissipation(0.01);
		contactParams.setDynamicFriction(0.25);
		contactForce->updContactParametersSet().append(contactParams);
		contactForce->setName("contactForce");
		osimModel.addForce(contactForce);

		double time[2]={0,finalTime};
		double fXofT[2]={0,-blockMass*9.80665*3.0};
		double pXofT[2]={0,0.1};

		PiecewiseLinearFunction *forceX=new PiecewiseLinearFunction(2,time,fXofT);
		PiecewiseLinearFunction *pointX=new PiecewiseLinearFunction(2,time,pXofT);

		PrescribedForce *prescribedForce=new PrescribedForce(block);
		prescribedForce->setName("prescribedForce");

		prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0));
		prescribedForce->setPointFunctions(pointX, new Constant(0.0), new Constant(0.0));

		osimModel.addForce(prescribedForce);


		osimModel.print("tugOfWar_model.osim");

		SimTK::State& si = osimModel.initSystem();

		CoordinateSet& modelCoordinateSet = osimModel.updCoordinateSet();
		modelCoordinateSet[3].setValue(si, blockSideLength);
		modelCoordinateSet[3].setSpeedValue(si,0.1);
		modelCoordinateSet[4].setValue(si,blockSideLength/2+0.01);

		

		Array<double> slopeAndIntercept1(0.0,2);
		Array<double> slopeAndIntercept2(0.0,2);
		slopeAndIntercept1[0]=-1.0/(finalTime-initialTime);
		slopeAndIntercept1[1]=1.0;
		slopeAndIntercept2[0]=1.0/(finalTime-initialTime);
		slopeAndIntercept2[1]=0.05;
			
		muscleController->prescribeControlForActuator("muscle1", new LinearFunction(slopeAndIntercept1));
		muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2));

		muscle1->setDefaultActivation(slopeAndIntercept1[1]);
		muscle2->setDefaultActivation(slopeAndIntercept2[1]);

		muscle1->setDefaultFiberLength(0.1);
		muscle2->setDefaultFiberLength(0.1);

		osimModel.computeEquilibriumForAuxiliaryStates(si);


		SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
		integrator.setAccuracy(1.0e-4);
		Manager manager(osimModel, integrator);

		manager.setInitialTime(initialTime);
		manager.setFinalTime(finalTime);
		std::cout << "\n\n Integrating from " << initialTime <<" to " <<finalTime<<std::endl;
		manager.integrate(si);

		Storage statesDegrees(manager.getStateStorage());
		statesDegrees.print("tugofwar_states.sto");
		osimModel.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
		statesDegrees.setWriteSIMMHeader(true);
		statesDegrees.print("tugofwar_states_degrees.mot");



	}
    catch (OpenSim::Exception ex)
    {
        std::cout << ex.getMessage() << std::endl;
        return 1;
    }
    catch (std::exception ex)
    {
        std::cout << ex.what() << std::endl;
        return 1;
    }
    catch (...)
    {
        std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
        return 1;
    }

	


    std::cout << "OpenSim example completed successfully.\n";
	std::cin.get();
	return 0;
}
